# -*- coding: utf-8 -*-
"""
Spyder Editor

This is a temporary script file.
"""
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation

class UAV:
    #UAV固定属性，包括导弹数量与巡航速度,初始航向
    #初始化无人机对象，包括名称与初始位置
    def __init__(self,UAV_name,Num_Missile=2,攻击半径=10,speed=0.3):
        self.name = UAV_name
        self.NumberOfMissile = Num_Missile
        self.Position = []  # x,y,z
        self.Speed = speed # 标量
        self.att_radius=攻击半径
        self.pointing=[]    # 机头朝向，[t,p]俯仰Theta，水平偏转是Phi
        self.v_missile=0.5
        self.live=1


    #无人机运动函数，三自由度运动函数，输入为当前航向，更新当前位置,并添加进无人机轨迹;dt 外部设置步长
    # TODO (zrc) 修改无人机运动方式，改为球坐标，
    def FlightTo(self,Theta,Phi,dt):
        """
        #三个方向的数值
        v1 = Direction[0]
        v2 = Direction[1]
        v3 = Direction[2]
        #归一化,相对运动矢量
        v_total = v1+v2+v3
        v1_p = v1/v_total
        v2_p = v2/v_total
        v3_p = v3/v_total
        d11 = self.Position[0]
        d12 = self.Position[1]
        d13 = self.Position[2] 
        #根据运动方向更新无人机坐标
        d11+=v1_p*dt*self.Speed
        d12+=v2_p*dt*self.Speed
        d13+=v3_p*dt*self.Speed
        self.Position[0] = d11
        self.Position[1] = d12
        self.Position[2] = d13
        #将坐标加入轨迹
        self.Position[0].append(d11)
        self.Position[1].append(d12)
        self.Position[2].append(d13)
        """
        assert Theta>=-math.pi/6 and Theta<=math.pi/6 , '输入角度Theta超出范围'
        assert Phi >= -math.pi / 6 and Theta <= math.pi / 6, '输入角度Phi超出范围'
        Theta+=self.pointing[0]
        Phi+=self.pointing[1]

        dx = self.Speed * math.sin(Theta) * math.cos(Phi)*dt
        dy = self.Speed * math.sin(Theta) * math.sin(Phi)*dt
        dz = self.Speed * math.cos(Theta)*dt

        self.Position[0]+=dx
        self.Position[1] += dy
        self.Position[2] += dz

    def Attack(self,pos):   # TODO （zrc） 验证是否有弹，有弹则可以攻击返回True
        r=self.distance_xyz(self.Position,pos)
        if self.NumberOfMissile > 0 and r<self.att_radius:
            self.NumberOfMissile -= 1
            return True
        else:
            return False

    def distance_xyz(self,A,B):
        x=B[0]-A[0]
        y = B[1] - A[1]
        z = B[2] - A[2]
        dis=math.sqrt(x**2+y**2+z**2)
        return dis
        
        
        
            
            
            